/**
 * @file kobuki_driver/packets/dock_ir.hpp
 *
 * @brief Docking infrared sensor packet payloads.
 *
 * License: BSD
 *   https://raw.github.com/yujinrobot/kobuki_core/hydro-devel/kobuki_driver/LICENSE
 */
/*****************************************************************************
** Preprocessor
*****************************************************************************/

#ifndef KOBUKI_DOCK_IR_DATA_HPP__
#define KOBUKI_DOCK_IR_DATA_HPP__

/*****************************************************************************
** Include
*****************************************************************************/

#include "../packet_handler/payload_headers.hpp"
#include "../packet_handler/payload_base.hpp"
/*****************************************************************************
** Namespace
*****************************************************************************/

/*****************************************************************************
** Interface
*****************************************************************************/

/*****************************************************************************
** 
ir 
near left
EF1F1509

near mid
989D2439

near left & near mid
4AF2AA20

near right & near mid
CC1C9F5A


near right
CC36FF5E
*****************************************************************************/

class DockIR : public payloadBase
{
public:
  DockIR() : payloadBase(false, 3) {};
  struct Data {
    Data() : docking(3) {}
    std::vector<uint8_t> docking;
  } data;

  enum IRFlag
  {
// 0x01 for NEAR_LEFT state
// 0x02 for NEAR_CENTER state
// 0x04 for NEAR_RIGHT state
// 0x08 for FAR_CENTER state
// ox10 for FAR_LEFT state
// 0x20 for FAR_RIGHT state
    NEAR_LEFT   = 0x01,
    NEAR_CENTER = 0x02,
    NEAR_RIGHT  = 0x04,
    FAR_CENTER  = 0x08,      
    FAR_LEFT    = 0x10,
    FAR_RIGHT   = 0x20,    
  };

  bool serialise(deque<unsigned char> & byteStream)
  {
    buildBytes((unsigned char)Header::DockInfraRed, byteStream);
    buildBytes(length, byteStream);
    buildBytes(data.docking[0], byteStream);
    buildBytes(data.docking[1], byteStream);
    buildBytes(data.docking[2], byteStream);
    return true;
  }

  bool deserialise(deque<unsigned char> & byteStream)
  {
    if (byteStream.size() < length+2)
    {
      //std::cout << "kobuki_node: kobuki_dock_ir: deserialise failed. not enough byte stream." << std::endl;
      return false;
    }

    unsigned char header_id, length_packed;
    buildVariable(header_id, byteStream);
    buildVariable(length_packed, byteStream);
    if( header_id != Header::DockInfraRed ) return false;
    if( length_packed != length ) return false;

    buildVariable(data.docking[0], byteStream);
    buildVariable(data.docking[1], byteStream);
    buildVariable(data.docking[2], byteStream);

    //showMe();
    return true;
  }

  void setIR(const long recv, const int type)
  {
    unsigned char ir;
    switch (recv) {
      case 0xEF1F1509://near left
      case 0x135D053C:
        ir = NEAR_LEFT;
        break;
      case 0x989D2439:
        ir = NEAR_CENTER;
        break;
      case 0xCC36FF5E:
      case 0xF1D8F649:      
        ir = NEAR_RIGHT;
        break;
      case 0x4AF2AA20:
        ir = (NEAR_LEFT | NEAR_CENTER);
        break;
      case 0xCC1C9F5A:
        ir = (NEAR_RIGHT | NEAR_CENTER);
        break;
      default:
        ir = 0;
    }

    data.docking[type] = ir;
  }
};

#endif /* KOBUKI_IR_DATA_HPP__ */
